// INTRINSICS
// D. TAYLOR 2014

#ifndef __INTRINSICS_H__
#define __INTRINSICS_H__
#include "config.h"

// reverse bits (ARM: RBIT)
/*__inline 
uint32_t rbit(uint32_t x) {
  uint32_t result;
  x = (((x & 0xaaaaaaaa) >> 1) | ((x & 0x55555555) << 1));
  x = (((x & 0xcccccccc) >> 2) | ((x & 0x33333333) << 2));
  x = (((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4));
  x = (((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8));
  result = (x >> 16) | (x << 16);
  return result;
}
*/
#define rbit __rbit

#define RBITS(W, BITS) (rbit(W) >> (32 - (BITS)))

/*// count leading zeroes (ARM: CLZ)
__inline 
uint32_t clz(uint32_t x) {
  uint32_t result;
  if(x == 0) return 32;
  result = 0;
  while(!(x & 0x80000000)) {
  result++;
  x <<= 1;
  }
  return result;
}*/
#define clz __clz

// 32-bit signed multiply -> 32-bit result, add 32-bit (ARM: SMMLAR)
int32_t smmlar(int32_t a, int32_t b, int32_t c) {
  int32_t result;
  result = ((((int64_t)a * b) + 0x80000000) >> 32) + c;
  return result;
}

// 32-bit signed multiply -> 32-bit result, subtract 32-bit (ARM: SMMLSR)
__inline 
int32_t smmlsr(int32_t a, int32_t b, int32_t c) {
  int32_t result;
  result = ((((int64_t)a * b) + 0x80000000) >> 32) - c;
  return result;
}

// 32-bit signed multiply -> 32-bit result (ARM: SMMULR)
__inline 
int32_t smmulr(int32_t a, int32_t b) {
  int32_t result;
  result = ((((int64_t)a * b) + 0x80000000) >> 32);
  return result;
}

// saturating add (ARM: qadd)
__inline 
int32_t qadd(int32_t a, int32_t b) {
  uint32_t result;
  int64_t c = (int64_t)a + b;
  if(c >  2147483647ll) c =  2147483647ll;
  if(c < -2147483648ll) c = -2147483648ll;
  result = c;
  return result;
}

// saturating subtract (ARM: qsub)
__inline 
int32_t qsub(int32_t a, int32_t b) {
  uint32_t result;
  int64_t c = (int64_t)a - b;
  if(c >  2147483647ll) c =  2147483647ll;
  if(c < -2147483648ll) c = -2147483648ll;
  result = c;
  return result;
}

// 32-bit arithmetic shift right with rounding (ARM: ASRS + ADC)
__inline 
int32_t asrr(int32_t v, int32_t s) {
  int32_t result;
  result = (v + (1 << (s - 1))) >> s;
  return result;
}

#endif
